#ifndef MAGNETICTRACK_H
#define MAGNETICTRACK_H
#include <QString>
#include <QVector>
#include "pid_controller.h"
#include "vehicledefine.h"
#include "gSoapFoundation/soapStub.h"
#include "MacroDefine/GeneralDefine.h"



//#define D_RFID_STOP_ID 1

struct SensorInfo
{
    QVector<bool> magneticData_front;
    QVector<bool> magneticData_back;
    long long rfidData_front;
    bool isRfidDetected_front;
    long long rfidData_back;
    bool isRfidDetected_back;
    double deltaDistance_virtualLine;
    double absoluteDistance_virtualLine;
    double deltaAngle_virtualLine;
    double absoluteAngle_virtualLine;
    bool input;
};

enum E_TRACK_LOOP_RESULT
{
    E_TRACK_LOOP_RESULT_NORMAL,
    E_TRACK_LOOP_RESULT_ERROR,
    E_TRACK_LOOP_RESULT_ZERO_HALT,
    E_TRACK_LOOP_RESULT_MOVING_HALT,
};


class AxisObject;
class VehicleKinematics;
class MotionMessage;

class MagneticTrack
{
public:
    MagneticTrack(QString fileName, MotionMessage *motionMessageIn);

public:
    int setVehicleDriveDirection(bool isPositiveDirectionIn);
    int getTrackStatus(E_TRACK_STATUS &statusOut);
    int setTrackMode(E_TRACK_MODE modeIn);
    int setStopCondition(E_TRACK_STOP_CONDITION conditionIn);
    int setMageneticPassRfid(long long passRfidIn);

    /**
     * @brief setVehicleIdealSpeed_ratio
     * @param speedIn :unit is %
     * @return
     */
    int setVehicleIdealSpeed_ratio(double speedIn);
    /**
     * @brief setVehicleIdealSpeed_mPerS
     * @param speedIn : unit is m/s
     * @return
     */
    int setVehicleIdealSpeed_mPerS(double speedIn);// m/s
    /**
     * @brief setVehicleIdealRotation_ratio
     * @param speedIn : unit is %
     * @return
     */
    int setVehicleIdealRotation_ratio(double speedIn);// %
    int stop();
    int cease();
    int resetAll();
    int startRun();
    bool isInitialOk();
    /**
     * @brief trackLoop
     * @param sensorInfo
     * @param wheelVelOut unit is deg/s
     * @return
     */
    E_TRACK_LOOP_RESULT trackLoop(SensorInfo sensorInfo,QVector<double>& wheelVelOut);
    int getErrorCode();
//    int resetError();
    int setMaxErrorCount(int maxCount);
    int setVehicleMagneticMovePid(double pid_p, double pid_i, double pid_d,
                                  double pid_velocity_p, double lineAdjustMax, double rotationAdjustMax);
    int setVirtualLineMovePid(double pid_p,double pid_i,double pid_d);
    int getPidOutput(double &pathPidOutput, double &velocityPidOutput);


private:

    int calculateVirtualLineIdealSpeed(double deltaDistanceIn, double &idealSpeedOut);
    int ultrasonicSensorProtectLoop(bool driveDirection, double &safeSpeedRatioOut);
    int readConfigFile(QString fileName);
    int initialPathPidController();
    int initialVelocityPidController();
    int pidOutput2WheelVel(double pidDriveValue, QVector<double> &wheelVelOut);//unit is deg/s
    int pidOutput2WheelVel_twoDifferentialDriver_line(double pidDriveValue, QVector<double> &wheelVelOut);
    int pidOutput2WheelVel_twoDifferentialDriver_angle(double pidDriveValue, QVector<double> &wheelVelOut);
    int pidOutput2WheelVel_oneSterringDriver_line(double pidDriveValue, QVector<double> &wheelVelOut);
    int pidOutput2WheelVel_oneSterringDriver_angle(double pidDriveValue, QVector<double> &wheelVelOut);
    int pidOutput2WheelVel_twoSterringDriver_line(double pidDriveValue, QVector<double> &wheelVelOut);
    int pidOutput2WheelVel_twoSterringDriver_angle(double pidDriveValue, QVector<double> &wheelVelOut);

    int pidInputCalculate(E_TRACK_MODE trackModeIn,bool isPositiveDirectionIn, SensorInfo sensorInfoIn,double &resultOut);
    int pidInputCalculate_maganetic_middle(QVector<bool> magneticData, double &resultOut);
    int pidInputCalculate_maganetic_low(QVector<bool> magneticData, double &resultOut);
    int pidInputCalculate_maganetic_high(QVector<bool> magneticData, double &resultOut);
    int pidInputCalculate_virtualLine(bool isPositiveDirectionIn, double absoluteAngle, double deltaAngle,
                                      double absoluteDistance, double &resultOut);
    E_STOP_STATUS passConditionLoop(SensorInfo sensorInfoIn);
    E_STOP_STATUS stopConditionLoop(SensorInfo sensorInfoIn,double &slowdownRatioOut);
    E_STOP_STATUS stopCondition_input(bool inputDi, double &slowdownRatioOut);
    E_STOP_STATUS stopCondition_rfid(bool isRfidDetected, long long rfidData, double &slowdownRatioOut);
    E_STOP_STATUS stopCondition_maganeticWidth(QVector<bool> magneticData,double &slowdownRatioOut);
    E_STOP_STATUS stopCondition_virtualLine(double deltaDistance,double deltaAngle,double &slowdownRatioOut);

    int initialSpeedRatioAxis();
    double getSpeedRatioAxisScale();
    int setSpeedRatioAxisScale(double ratio, double timeDuration);
    int speedRatioAxisMoveOneStep();
    int resetSpeedRatioAxis();


private:
    VehicleKinematics* vehicleKinematic;
    E_VEHICLE_TYPE vehicletype;
    double trackDirectionAngle;//双舵轮时，追踪路线与双舵轮连线的夹角。涉及到不同控制策略。
    bool initialFlag;
    double lastSensorInfo;
    double sensorTotalWidth;
    double magneticPidParameter_P;
    double magneticPidParameter_I;
    double magneticPidParameter_D;
    double magneticLineSpeedAdjustMax;//m/s,//单轮偏差调整最大值
    double magneticAngleSpeedAdjustMax;//degree/s 旋转运动最大偏差补偿
    double magneticVehicleVelocityConpensation;//速度比例补偿

    double lineSpeedAdjustMax;//m/s,//单轮偏差调整最大值
    double angleSpeedAdjustMax;//degree/s 旋转运动最大偏差补偿
    double vehicleVelocityConpensation;//速度比例补偿

    double sampleTime;
    int frontMaganeticDirection;
    int backMaganeticDirection;


    PIDControl *pathPidController;
    PIDControl *velocityPidController;

    int errorCount;
    int maxErrorCount;

    E_TRACK_STATUS status;
    double idealSpeed;// unit m/s
    double composedLineSpeed;//与减速速率合成后的速度
    double idealRotationSpeed;//unit is rad/s
    double composedRotationSpeed;
    double slowdownSpeedRatio;//减速速率
    double safeSpeedRatio;//障碍物安全减速速率

    VehicleVelocity vehicleMaxVel;
    double wheelDiameter;
    double wheelDistance;
    double wheelMaxSpeed;

    bool isPositiveDirection;
    bool isLineMove;
    E_TRACK_MODE trackMode;
    E_TRACK_STOP_CONDITION stopCondition;

    double  allowedReachOffset_line;//M
    double  allowedReachOffset_angle;//degree

    AxisObject* speedRatioAxis;//速度比例轴
    int stopStage;
    MotionMessage *motionMessage;
    int maganeticWidthCount_stop;//停止磁感应灯个数
    double slowdownRange_line;
    double slowdownRange_angle;

    int rfidSensorCount;
    int maganeticSensorCount;
    long long passRfid;

    double velPidParameter_P;
    double velPidParameter_I;
    double velPidParameter_D;
    double virtualLinePathPidParameter_P;
    double virtualLinePathPidParameter_I;
    double virtualLinePathPidParameter_D;
    double virtualLinePathLineSpeedAdjustMax;//直线偏差调整最大值
    double virtualLineAngleSpeedAdjustMax;//旋转运动最大偏差补偿
    double virtualLinePathVehicleVelocityConpensation;//速度比例补偿

};

#endif // MAGNETICTRACK_H
